#! /usr/bin/env python

# Copyright (c) 2010, Arizona Robotics Research Group, University of Arizona
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# 
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Willow Garage, Inc. nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS'
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Matt Brockman

PKG = 'test_manipulation'
NAME = 'drop_service'

#copied from move_arm_demo
import roslib; roslib.load_manifest(PKG)
import rospy

from cs665_manipulation_srvs.srv import *
from actionlib import SimpleActionClient
from wubble_actions.msg import *
import math

class DropService():

    def __init__(self):
        rospy.init_node(NAME + 'server' , anonymous=True)
        self.client = SimpleActionClient("smart_arm_action", SmartArmAction)
        self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
        self.client.wait_for_server()
        self.gripper_client.wait_for_server()
        self.service = rospy.Service('drop_service', ArmDropService, self.handle_drop)
        rospy.loginfo("%s: Ready for Dropping", NAME)

    #drop it like it's hot
    def handle_drop(self, req):
        rospy.loginfo("told to drop")
        success = False
        #this seems to be as far as it can open (both straight ahead), same vals anh uses
        if self.move_gripper(0.2, -0.2):
            success = True      
        return success

    #stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks 
    def move_gripper(self, left_finger, right_finger):
        goal = SmartArmGripperGoal()
        goal.target_joints = [left_finger, right_finger]

        self.gripper_client.send_goal(goal)
        self.gripper_client.wait_for_goal_to_finish()

        result = self.gripper_client.get_result()
        if result.success == False:
            rospy.loginfo("Drop failed")
        else:
            rospy.loginfo("Object Released!  Hopefully it isn't sticky")

if __name__ == '__main__':
    try:
        pointle = DropService()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
